#include "world.h"


world::world(void)
{

	//squadCount = 0;
	//maxSquads = 100;
	//structureCount = 0;
	structureMax = 100;

	bluePopulation = redPopulation = 0;
	blueSupplies = redSupplies = 20;
	
	goalActive = false;
	goal = vertex(0,0);

	map = Terrain();
	//map.Load_Test_Map_1();

	resource_spawn_counter = 0;

	triRoadCost = 1.0;
	triDirtCost = ((float)TRI_DIRT_LIMIT) / ((float)TRI_GRAVEL_LIMIT);
	triGravelCost = ((float)TRI_ROAD_LIMIT) / ((float)TRI_DIRT_LIMIT);
	rectRoadCost = 1.0;
	rectDirtCost = ((float)RECT_DIRT_LIMIT) / ((float)RECT_GRAVEL_LIMIT);
	rectGravelCost = ((float)RECT_ROAD_LIMIT) / ((float)RECT_DIRT_LIMIT);
	circRoadCost = 1.0;
	circDirtCost = ((float)CIRC_DIRT_LIMIT) / ((float)CIRC_GRAVEL_LIMIT);
	circGravelCost = ((float)CIRC_ROAD_LIMIT) / ((float)CIRC_DIRT_LIMIT);

	destroyList.clear();
}


world::~world(void)
{
}

/*
int world::getSquadCount(void) { return squadCount;}

squad * world::fetchSquad(int index) {return squadList[index];}
*/

int world::addSquad(squad * newSquad) {
	int totalCost = newSquad->maxAgents*newSquad->unitPopCost;

	if (newSquad->team == BLUE) {
		if (totalCost+bluePopulation <= blueSupplies) {
			squadList.push_back(newSquad);
			bluePopulation += totalCost;
			return 0;
		}
	}
	else {
		if (totalCost+redPopulation <= redSupplies) {
			squadList.push_back(newSquad);
			redPopulation += totalCost;
			return 0;
		}
	}

	return 1;
}

/*
int world::getStructureCount(void) { return structureCount;}

structure * world::fetchStructure(int index) {return structureList[index];}
*/

int world::addStructure(structure * newObst) {
	if (structureList.size() < structureMax) {
		structureList.push_back(newObst);
		return 0;
	}
	else return 1;
}

void world::resetSquads(void) {
	squad * currentSquad;
	while (! squadList.empty()) {
		currentSquad = squadList.back();
		delete currentSquad;
		squadList.pop_back();
	}
}

void world::resetStructures(void) {
	structure * currentStructure;
	while (! structureList.empty()) {
		currentStructure = structureList.back();
		delete currentStructure;
		structureList.pop_back();
	}
}

void world::updateSquadAgents(int squadIndex, time_t diffT, FILE * outputFile) {

	int i,j,k;
	float distance;
	vertex targetPos;
	agent * currentAgent, * otherAgent;
	squad * currentSquad = squadList[squadIndex];
	squad * otherSquad;
	structure * otherStructure;
	bool candidateIsAgent;
	int agentCount = currentSquad->agentList.size();
	int oppAgentCount;
	AVector sepVector, avoidVector, cohVector, alignVector, borderVector, obstacleVector, goalVector;

	int totalDestroyed = 0;

	for (i=0; i < agentCount; i++) {

		// Load the currently considered agent:
		currentAgent = currentSquad->agentList[i];

		// Search for attack opportunities only if squad is aggressive
		if (currentSquad->aggressive) {

			// Perform target checking functions only if agent doesn't already have a target
			if (currentAgent->target.targetObj == 0) {

				currentAgent->target.distance = numeric_limits<float>::infinity();

				// If squad is attacking...
				if ( currentSquad->attacking) {
					// Check each squadmate (not including yourself)
					for (j = 0; j < agentCount; j++) {
						if (j != i) {
							otherAgent = currentSquad->agentList[j];
							// If this squadmate has a target, see if it's the closest available target
							if (otherAgent->target.targetObj != 0) {
								if (otherAgent->target.isAgent) {
									targetPos = ((agent*)otherAgent->target.targetObj)->position;
									currentAgent->target.isAgent = true;
								}
								else {
									// TODO: should the agent target the structure's exact position?
									targetPos = ((structure*)otherAgent->target.targetObj)->position;
									currentAgent->target.isAgent = false;
								}
								distance = currentAgent->position.distance(targetPos);
								if (distance < currentAgent->target.distance) {
									currentAgent->target.distance = distance;
									currentAgent->target.targetObj = otherAgent->target.targetObj;
								}
							}
						}

						
					}

					// Squad status correction: if no agents actually have a target, the first agent
					// on the list will reset the squad attack status
					if (currentAgent->target.targetObj == 0) currentSquad->attacking = false;
					else {
						// Check if chosen target is in attack range
						// NOTE: This depends on if the target is a unit or not
						//       If target is a unit, simply see if it is within this unit's attack range
						//       If target is a structure, subtract the structure radius from the distance
						if (currentAgent->target.isAgent)
							currentAgent->target.inRange = (currentAgent->target.distance <= currentSquad->attackRange);
						else {
							float modifiedDistance = currentAgent->target.distance - ((structure*)currentAgent->target.targetObj)->radius;
							currentAgent->target.inRange = (modifiedDistance <= currentSquad->attackRange);
						}
					}
				}
				// Squad is not currently attacking
				else {
					void * targetCandidate;
					// Check for possible opponent squads in attack range
					for (j=0; j < squadList.size(); j++) {
						// Fetch next squad on list
						otherSquad = squadList[j];
						// If other squad is on opposing team...
						if (currentSquad->team != otherSquad->team) {
							oppAgentCount = otherSquad->agentList.size();
							// Check the distance of each opposing agent
							for (k=0; k < oppAgentCount; k++) {
								otherAgent = otherSquad->agentList[k];
								distance = currentAgent->position.distance(otherAgent->position);
								if (distance < currentAgent->target.distance) {
									targetCandidate = (void*) otherAgent;
									candidateIsAgent = true;
									currentAgent->target.distance = distance;
								}
							}
						}
					}
					// Check for possible opponent structures in attack range
					for (j=0; j < structureList.size(); j++) {
						// fetch next structure on list
						otherStructure = structureList[j];
						// If structure belongs to opposing team...
						if (currentSquad->team != otherStructure->team) {
							//Note the structure's distance (compensate for radius)
							distance = currentAgent->position.distance(otherStructure->position);
							distance -= otherStructure->radius;
							//Check if it is closer than the last closest target
							if (distance < currentAgent->target.distance) {
								targetCandidate = (void*) otherStructure;
								candidateIsAgent = false;
								currentAgent->target.distance = distance;
							}
						}
					}
					// Check if chosen target (unit or structure) is in attack range. If not, ignore
					if (currentAgent->target.distance <= currentSquad->attackRange) {
						currentAgent->target.targetObj = targetCandidate;
						currentAgent->target.isAgent = candidateIsAgent;
						currentAgent->target.inRange = true;
						currentSquad->attacking = true;
					}
				}
			}
			//  Agent already has a target. Update the distance and check if it's in range
			else {
				targetPos = currentAgent->getTargetPosition();
				currentAgent->target.distance = currentAgent->position.distance(targetPos);
				if (currentAgent->target.isAgent)
					currentAgent->target.inRange = (currentAgent->target.distance <= currentSquad->attackRange);
				else {
					float modifiedDistance = currentAgent->target.distance - ((structure*)currentAgent->target.targetObj)->radius;
					currentAgent->target.inRange = (modifiedDistance <= currentSquad->attackRange);
				}
			}

		}
		else {
			currentAgent->target.targetObj = 0;
			currentAgent->target.inRange = false;
		}

		// If agent is not in range of attack target, apply normal set of non-goal boids
		if (!currentSquad->aggressive || ! currentAgent->target.inRange) {
			
			///////////POST VERSION 24: rearranged movement logic for breaking
			bool halted = false;
			// If unit has a target, apply avoidance forces and target goal force
			if ( currentAgent->target.targetObj != 0) {

				// Avoidance-based forces
				avoidVector = agentAvoidForce(currentSquad, squadIndex, i);
				borderVector = borderForce(currentSquad, i, -100, 100, -100, 100);
				obstacleVector = obstacleForce(currentSquad, i);

				// Target-based force
				goalVector = targetForce(currentSquad, currentAgent);

				//Update velocity
				currentAgent->velocity = currentAgent->velocity + avoidVector + borderVector +
					                     obstacleVector + goalVector;
			}
			// If unit does not have a target, but has a path goal, apply both boids and avoidance
			// forces, along with path goal force
			else if (!currentSquad->goalList.empty()) {
				// Boids forces
				sepVector = seperationForce(currentSquad, i);
				cohVector = cohesionForce(currentSquad, i);
				alignVector = alignmentForce(currentSquad, i);
				// Avoidance forces
				avoidVector = agentAvoidForce(currentSquad, squadIndex, i);
				borderVector = borderForce(currentSquad, i, -100, 100, -100, 100);
				obstacleVector = obstacleForce(currentSquad, i);
				// Path goal force
				goalVector = goalForce(currentSquad, i);

				//Update velocity
				currentAgent->velocity = currentAgent->velocity + sepVector + cohVector + alignVector +
					                     avoidVector + borderVector + obstacleVector + goalVector;
			}
			// If no targets or goals are to be had, hit the breaks and stop
			// (slow down if moving, or remain standing if still)
			else {
				float speed = currentAgent->velocity.length();
				// Reduce velocity if unit is still moving
				if (speed != 0) {
					// Check to see if agent has slowed down to a low threshold. If so, just stop the unit.
					if (speed < 0.002) 
						currentAgent->velocity = AVector(0,0);
					// Else apply slowing modifier to velocity
					else 
						currentAgent->velocity = currentAgent->velocity * 0.99;
				}
				else
					halted = true;
			}
			//Limit speed, and update direction/position so long as the unit hasn't halted
			if (!halted) {
				// Limit the speed of the current agent
				limitSpeed(currentSquad, currentAgent);

				// Update direction and position of agent
				if (currentAgent->velocity.length() != 0)          // Only update direction if there exists a velocity
					currentAgent->direction = currentAgent->velocity.getDirection();

				// Add velocity to agent position
				currentAgent->position = currentAgent->position + (currentAgent->velocity * (float)diffT);
			}
			///////////////////////////END POST VERSION 24 LOGIC
			/*
			///////////////////////////BEGIN PRE VERSION 24 LOGIC
			
			// Calculate each relevant force for each agent
			avoidVector = agentAvoidForce(currentSquad, squadIndex, i);
			borderVector = borderForce(currentSquad, i, -100, 100, -100, 100);
			obstacleVector = obstacleForce(currentSquad, i);

			// Update the current Agent's velocity
			currentAgent->velocity = currentAgent->velocity + avoidVector +  
									 borderVector + obstacleVector;

			// Determine goal force depending on target availability
			// If there is no target, apple squad goal force (if applicable)
			if ( currentAgent->target.targetObj == 0) {

				// These forces should only be applied if no target is being persued (?)
				sepVector = seperationForce(currentSquad, i);
				cohVector = cohesionForce(currentSquad, i);
				alignVector = alignmentForce(currentSquad, i);
				currentAgent->velocity = currentAgent->velocity + sepVector + cohVector + alignVector;

				// Apply goal force if squad has a goal active
				if (!currentSquad->goalList.empty()) {
					goalVector = goalForce(currentSquad, i);
					currentAgent->velocity = currentAgent->velocity + goalVector;
				}

			}
			// There is a target, and the agent should float to it
			else {
				goalVector = targetForce(currentSquad, currentAgent);
				currentAgent->velocity = currentAgent->velocity + goalVector;
			}

			// Limit the speed of the current agent
			limitSpeed(currentSquad, currentAgent);

			// Update direction and position of agent
			if (currentAgent->velocity.length() != 0)          // Only update direction if there exists a velocity
				currentAgent->direction = currentAgent->velocity.getDirection();

			// Add velocity to agent position
			currentAgent->position = currentAgent->position + (currentAgent->velocity * (float)diffT);
			///////////////////////////END PRE VERSION 24 LOGIC
			*/
		}
		// If the target is in range, halt for attack
		else {
			currentSquad->attacking = true;
			currentAgent->velocity = AVector(0,0);
			targetPos = currentAgent->getTargetPosition();
			currentAgent->direction = (targetPos - currentAgent->position).getDirection();

			// Apply attack
			bool destroyed;
			if (currentAgent->atkTimer >= currentAgent->atkInterval) {
				destroyed = currentAgent->atkTarget();
				currentAgent->atkTimer = 0;

				switch (currentSquad->team) {
				case RED:
					switch (currentAgent->type) {
					case TRIANGLE:
						cout << "ATTACK - From Red Triangle" << endl;
						break;
					case CIRCLE:
						cout << "ATTACK - From Red Circle" << endl;
						break;
					case RECTANGLE:
						cout << "ATTACK - From Red Rectangle" << endl;
						break;
					}
					break;
				case BLUE:
					switch (currentAgent->type) {
					case TRIANGLE:
						cout << "ATTACK - From Blue Triangle" << endl;
						break;
					case CIRCLE:
						cout << "ATTACK - From Blue Circle" << endl;
						break;
					case RECTANGLE:
						cout << "ATTACK - From Blue Rectangle" << endl;
						break;
					}
					break;
				}

				// If target is destroyed by this attack, add target to destroyList
				if (destroyed) {
					destroyList.push_back(currentAgent->target.targetObj);
					totalDestroyed++;
					fputs("Object destroyed. Total destroyed this round: ", outputFile);
					char str[10];
					sprintf(str, "%d", totalDestroyed);
					fputs(str,outputFile); fputs("\n",outputFile);
					cout << "DESTROYED!!" << endl;
				}
			}
		}
		if (currentAgent->atkTimer < currentAgent->atkInterval)
			currentAgent->atkTimer += diffT;

		attemptResourcePickup(currentAgent->position, currentAgent->team);
	}

	// Squad goal-related actions under Explore mode
	if (currentSquad->isExploring()) {
		// If explore timer has expired, start new goal
		if (currentSquad->expTimer >= currentSquad->expInterval) {
			currentSquad->expTimer = 0;
			currentSquad->activateExplore(&map);
		}
		// Else update the timer and do normal goal checks
		else {
			currentSquad->expTimer += diffT;
			// If goal is updated, the list is now empty (since there's only one goal in explore mode)
			// Restart the goal
			if (currentSquad->updateGoal(&map)) 
					currentSquad->activateExplore(&map);
		}
	}
	// Squad goal-related actions while NOT under explore mode:
	// GOAL CHECKING - If squad goal is active, do the following:
	// * Check if the squad has collectively reached a goal waypoint. If so, update the waypoint
	// * If not, check to see if the squad wound up closer to the next waypoint than the current one
	//   --> If so, discard the current waypoint early
	else if (!currentSquad->goalList.empty()) {
		if (currentSquad->updateGoal(&map)) {
			currentSquad->goalList.pop_front();
		}
	}
}

// Based on http://www.openprocessing.org/sketch/8676
AVector world::seperationForce(squad * currentSquad, int agentIndex) {

	int i;
	int agentCount = currentSquad->agentList.size();
	agent * currentAgent = currentSquad->agentList[agentIndex];
	agent * otherAgent;
	float distance;
	AVector tempVector;
	AVector returnVector = AVector();  //initialized to (0,0) vector

	for (i = 0; i < agentCount; i++) {
		otherAgent = currentSquad->agentList[i];
		tempVector = currentAgent->position - otherAgent->position;
		distance = tempVector.length();
		if (i != agentIndex && distance < currentSquad->sepRange) {
			tempVector = tempVector.normalize();
			tempVector = tempVector / distance;

			returnVector = returnVector + tempVector;
		}
	}

	return returnVector * currentSquad->sepModifier;
}

// Forces for avoiding agents from other squads
AVector world::agentAvoidForce(squad * currentSquad, int squadIndex, int agentIndex) {

	int i,j;
	int agentCount;
	int squadCount = squadList.size();
	agent * currentAgent = currentSquad->agentList[agentIndex];
	squad * otherSquad;
	agent * otherAgent;
	float distance;
	AVector tempVector;
	AVector returnVector = AVector();  //initialized to (0,0) vector

	for (i = 0; i < squadCount; i++) {
		if (i != squadIndex) {
			otherSquad = squadList[i];
			agentCount = otherSquad->agentList.size();
			for (j = 0; j < agentCount; j++) {
				otherAgent = otherSquad->agentList[j];
				tempVector = currentAgent->position - otherAgent->position;
				distance = tempVector.length();
				if (distance < currentSquad->avoidRange) {
					tempVector = tempVector.normalize();
					tempVector = tempVector / distance;

					returnVector = returnVector + tempVector;
				}
			}
		}
	}

	return returnVector * currentSquad->avoidModifier;
}

// Based on http://www.openprocessing.org/sketch/8676
AVector world::cohesionForce(squad * currentSquad, int agentIndex) {

	int i;
	int encounters = 0;
	int agentCount = currentSquad->agentList.size();
	agent * currentAgent = currentSquad->agentList[agentIndex];
	agent * otherAgent;
	float distance;
	AVector tempVector;
	AVector returnVector = AVector();  //initialized to (0,0) vector

	for (i = 0; i < agentCount; i++) {
		otherAgent = currentSquad->agentList[i];
		tempVector = currentAgent->position - otherAgent->position;
		distance = tempVector.length();
		if (i != agentIndex && distance < currentSquad->cohRange) {
			returnVector = returnVector + AVector(otherAgent->position.x, otherAgent->position.y);
			encounters++;
		}
	}

	if (encounters > 0) {
		returnVector = returnVector / (float)encounters;
		returnVector = returnVector - AVector(currentAgent->position.x, currentAgent->position.y);
		returnVector = returnVector * currentSquad->cohModifier;
	}

	return returnVector;
}

// Based on http://www.openprocessing.org/sketch/8676
AVector world::alignmentForce(squad * currentSquad, int agentIndex) {

	int i;
	int encounters = 0;
	int agentCount = currentSquad->agentList.size();
	agent * currentAgent = currentSquad->agentList[agentIndex];
	agent * otherAgent;
	float distance;
	AVector tempVector;
	AVector returnVector = AVector();  //initialized to (0,0) vector

	for (i = 0; i < agentCount; i++) {
		otherAgent = currentSquad->agentList[i];
		tempVector = currentAgent->position - otherAgent->position;
		distance = tempVector.length();
		if (i != agentIndex && distance < currentSquad->aliRange) {
			returnVector = returnVector + otherAgent->velocity;
			encounters++;
		}
	}

	if (encounters > 0) {
		returnVector = returnVector / (float)encounters;
		returnVector = returnVector - currentAgent->velocity;
		returnVector = returnVector * currentSquad->aliModifier;
	}

	return returnVector;
}

AVector world::borderForce(squad * currentSquad, int agentIndex, float minx, float maxx, float miny, float maxy) {

	float minDist, maxDist;
	float posx, posy;
	float outx = 0, outy = 0;
	agent * currentAgent = currentSquad->agentList[agentIndex];
	AVector returnVector;

	posx = currentAgent->position.x;
	posy = currentAgent->position.y;

	maxDist = abs(posx - maxx); 
	minDist = abs(posx - minx);
	if (posx >= maxx) outx = (float)-.1;
	else if (posx <= minx) outx = (float).1;
	else if (maxDist < currentSquad->borRange) outx = (-1/maxDist) * (float)currentSquad->borModifier;
	else if (minDist < currentSquad->borRange) outx = (1/minDist) * (float)currentSquad->borModifier;

	maxDist = abs(posy - maxy); 
	minDist = abs(posy - miny);
	if (posy >= maxy) outy = (float)-.1;
	else if (posy <= miny) outy = (float).1;
	else if (maxDist < currentSquad->borRange) outy = (-1/maxDist) * (float)currentSquad->borModifier;
	else if (minDist < currentSquad->borRange) outy = (1/minDist) * (float)currentSquad->borModifier;

	returnVector = AVector(outx, outy);

	return returnVector;

}

AVector world::obstacleForce(squad * currentSquad, int agentIndex) {

	int i;
	int structureCount = structureList.size();
	float distance;
	agent * currentAgent = currentSquad->agentList[agentIndex];
	structure * currentStructure;
	float radius;
	AVector tempVector;
	AVector returnVector = AVector();

	for (i = 0; i < structureCount; i++) {
		currentStructure = structureList[i];
		radius = currentStructure->radius;
		tempVector = currentAgent->position - currentStructure->position;
		distance = tempVector.length();

		if (distance <= radius) {
			tempVector = tempVector.normalize() * (float)0.1;
			returnVector = returnVector + tempVector;
		}
		else if (distance < currentSquad->obsRange * radius) {
			tempVector = tempVector.normalize() * (1/distance) * currentSquad->obsModifier;
			returnVector = returnVector + tempVector;
		}
	}

	return returnVector;
}

// Based on http://www.openprocessing.org/sketch/8676
AVector world::goalForce(squad * currentSquad,  int agentIndex) {

	float distance;
	agent * currentAgent = currentSquad->agentList[agentIndex];
	AVector returnVector = AVector();  //initialized to (0,0) vector

	float goalRange = currentSquad->goalRange;
	float goalModifier = currentSquad->goalModifier;

	if (!currentSquad->goalList.empty()) {
		vertex currentGoal = currentSquad->goalList.front();
		distance = (currentAgent->position - currentGoal).length();
		if (distance < goalRange) {
			(currentGoal.x >= currentAgent->position.x) ? returnVector.x += (float)((goalRange - distance) * goalModifier) :
				                                   returnVector.x -= (float)((goalRange - distance) * goalModifier) ;
			(currentGoal.y >= currentAgent->position.y) ? returnVector.y += (float)((goalRange - distance) * goalModifier) :
				                                   returnVector.y -= (float)((goalRange - distance) * goalModifier) ;
		}
	}
	

	return returnVector;
}

AVector world::targetForce(squad * currentSquad, agent * currentAgent) {

	AVector returnVector = AVector();  //initialized to (0,0) vector
	float goalRange = currentSquad->goalRange;
	float goalModifier = currentSquad->goalModifier;
	vertex currentGoal = currentAgent->getTargetPosition();

	if (currentAgent->target.distance < goalRange) {
		(currentGoal.x >= currentAgent->position.x) ? returnVector.x += (float)((goalRange - currentAgent->target.distance) * goalModifier) :
				                                      returnVector.x -= (float)((goalRange - currentAgent->target.distance) * goalModifier) ;
		(currentGoal.y >= currentAgent->position.y) ? returnVector.y += (float)((goalRange - currentAgent->target.distance) * goalModifier) :
				                                      returnVector.y -= (float)((goalRange - currentAgent->target.distance) * goalModifier) ;
	}

	return returnVector;
}

void world::limitSpeed(squad * currentSquad, agent * currentAgent) {
	
	switch(map.GetTerrainType(currentAgent->position))
	{
		case Terrain::DIRT:
			switch(currentAgent->type)
			{
				case CIRCLE:
					currentAgent->speedLimit = TRI_DIRT_LIMIT;
					break;

				case TRIANGLE:
					currentAgent->speedLimit = CIRC_DIRT_LIMIT;
					break;

				case RECTANGLE:
					currentAgent->speedLimit = RECT_DIRT_LIMIT;
					break;
			}
			break;
		case Terrain::ROAD:
			switch(currentAgent->type)
			{
				case CIRCLE:
					currentAgent->speedLimit = TRI_ROAD_LIMIT;
					break;

				case TRIANGLE:
					currentAgent->speedLimit = CIRC_ROAD_LIMIT;
					break;

				case RECTANGLE:
					currentAgent->speedLimit = RECT_ROAD_LIMIT;
					break;
			}
			break;
		case Terrain::GRAVEL:
			switch(currentAgent->type)
			{
				case CIRCLE:
					currentAgent->speedLimit = TRI_GRAVEL_LIMIT;
					break;

				case TRIANGLE:
					currentAgent->speedLimit = CIRC_GRAVEL_LIMIT;
					break;

				case RECTANGLE:
					currentAgent->speedLimit = RECT_GRAVEL_LIMIT;
					break;
			}
			break;

		default:
			break;
	}


	if (currentAgent->velocity.length() > currentAgent->speedLimit) {
		currentAgent->velocity = currentAgent->velocity.normalize() * currentAgent->speedLimit;
	}
}

bool world::inList(vector<mapNode*> * nodeList, mapNode * inNode) {
	int i;
	int size = nodeList->size();

	for (i = 0; i < size; i++) {
		if (*inNode == *nodeList->at(i)) return true;
	}
	return false;
}

// Assumes two input nodes are both neighbors (adjacent or diagonal)
float world::costBetween(shape_T type, mapNode * node1, mapNode * node2) {

	Terrain::TerrainType terrain1 = map.GetTerrainType(node1->y,node1->x);
	Terrain::TerrainType terrain2 = map.GetTerrainType(node2->y, node2->x);

	bool diffX, diffY;
	float distance;
	float tWeight1, tWeight2;

	// Determine if neighbors are diagonal or adjacent
	diffX = (bool)abs(node1->x - node2->x);
	diffY = (bool)abs(node1->y - node2->y);
	if (diffX && diffY) distance = sqrt((float)2*(STEP*STEP));
	else distance = STEP;

	// Determine node weights based on terrain and agent type combos

	switch (type) {
	case TRIANGLE:
		switch (terrain1) {
		case Terrain::DIRT:
			tWeight1 = triDirtCost;
			break;
		case Terrain::ROAD:
			tWeight1 = triRoadCost;
			break;
		case Terrain::GRAVEL:
			tWeight1 = triGravelCost;
			break;
		case Terrain::OCCUPIED:
			tWeight1 = 10000;
			break;
		}
		switch (terrain2) {
		case Terrain::DIRT:
			tWeight2 = triDirtCost;
			break;
		case Terrain::ROAD:
			tWeight2 = triRoadCost;
			break;
		case Terrain::GRAVEL:
			tWeight2 = triGravelCost;
			break;
		case Terrain::OCCUPIED:
			tWeight2 = 10000;
			break;
		}
		break;

	case CIRCLE:
		switch (terrain1) {
		case Terrain::DIRT:
			tWeight1 = circDirtCost;
			break;
		case Terrain::ROAD:
			tWeight1 = circRoadCost;
			break;
		case Terrain::GRAVEL:
			tWeight1 = circGravelCost;
			break;
		case Terrain::OCCUPIED:
			tWeight1 = 10000;
			break;
		}
		switch (terrain2) {
		case Terrain::DIRT:
			tWeight2 = circGravelCost;
			break;
		case Terrain::ROAD:
			tWeight2 = circRoadCost;
			break;
		case Terrain::GRAVEL:
			tWeight2 = circGravelCost;
			break;
		case Terrain::OCCUPIED:
			tWeight2 = 10000;
			break;
		}
		break;

	case RECTANGLE:
		switch (terrain1) {
		case Terrain::DIRT:
			tWeight1 = rectDirtCost;
			break;
		case Terrain::ROAD:
			tWeight1 = rectRoadCost;
			break;
		case Terrain::GRAVEL:
			tWeight1 = rectGravelCost;
			break;
		case Terrain::OCCUPIED:
			tWeight1 = 10000;
			break;
		}
		switch (terrain2) {
		case Terrain::DIRT:
			tWeight2 = rectDirtCost;
			break;
		case Terrain::ROAD:
			tWeight2 = rectRoadCost;
			break;
		case Terrain::GRAVEL:
			tWeight2 = rectGravelCost;
			break;
		case Terrain::OCCUPIED:
			tWeight2 = 10000;
			break;
		}
		break;
	}

	/*switch (type) {
	case TRIANGLE:
		switch (terrain1) {
		case Terrain::DIRT:
			tWeight1 = TRI_DIRT_LIMIT;
			break;
		case Terrain::ROAD:
			tWeight1 = TRI_ROAD_LIMIT;
			break;
		case Terrain::GRAVEL:
			tWeight1 = TRI_GRAVEL_LIMIT;
			break;
		}
		switch (terrain2) {
		case Terrain::DIRT:
			tWeight2 = TRI_DIRT_LIMIT;
			break;
		case Terrain::ROAD:
			tWeight2 = TRI_ROAD_LIMIT;
			break;
		case Terrain::GRAVEL:
			tWeight2 = TRI_GRAVEL_LIMIT;
			break;
		}
		break;

	case CIRCLE:
		switch (terrain1) {
		case Terrain::DIRT:
			tWeight1 = CIRC_DIRT_LIMIT;
			break;
		case Terrain::ROAD:
			tWeight1 = CIRC_ROAD_LIMIT;
			break;
		case Terrain::GRAVEL:
			tWeight1 = CIRC_GRAVEL_LIMIT;
			break;
		}
		switch (terrain2) {
		case Terrain::DIRT:
			tWeight2 = CIRC_DIRT_LIMIT;
			break;
		case Terrain::ROAD:
			tWeight2 = CIRC_ROAD_LIMIT;
			break;
		case Terrain::GRAVEL:
			tWeight2 = CIRC_GRAVEL_LIMIT;
			break;
		}
		break;

	case RECTANGLE:
		switch (terrain1) {
		case Terrain::DIRT:
			tWeight1 = RECT_DIRT_LIMIT;
			break;
		case Terrain::ROAD:
			tWeight1 = RECT_ROAD_LIMIT;
			break;
		case Terrain::GRAVEL:
			tWeight1 = RECT_GRAVEL_LIMIT;
			break;
		}
		switch (terrain2) {
		case Terrain::DIRT:
			tWeight2 = RECT_DIRT_LIMIT;
			break;
		case Terrain::ROAD:
			tWeight2 = RECT_ROAD_LIMIT;
			break;
		case Terrain::GRAVEL:
			tWeight2 = RECT_GRAVEL_LIMIT;
			break;
		}
		break;
	}*/

	return ((tWeight1 + tWeight2) / 2) * distance;
	//return ((-tWeight1 - tWeight2) / 2);

}

list<vertex> world::AStarPlan(squad * currentSquad, vertex goalVert) {

	int i;
	int neighborSize;
	float tempGScore;
	mapNode * startNode, * goalNode;
	mapNode * currentNode, * neighborNode;
	int xRef, yRef;
	bool heapFlag;
	list<vertex> pathList;

	// Lists used for open/closed sets
	vector<mapNode*> openSet;
	vector<mapNode*> closedSet;
	vector<mapNode*> neighborList;

	//Initialize data structures
	startNode = new mapNode();
	//*goalNode = currentSquad->goalPosition(&map);
	vertex temp = map.GetTerrainPoint(goalVert);
	goalNode = new mapNode(temp.x, temp.y);
	*startNode = currentSquad->mapPosition(&map);
	startNode->gScore = 0;
	startNode->hScore = startNode->heuristic(goalNode, HEURISTICMOD);
	startNode->fScore = startNode->gScore + startNode->hScore;
	startNode->parent = 0;

	openSet.clear();
	openSet.push_back(startNode);
	make_heap(openSet.begin(), openSet.end(), H_Compare());  //Heap-based Priority queue

	closedSet.clear();
	neighborList.clear();

	// While openSet is not empty
	while (!openSet.empty()) {
		heapFlag = false;

		// Pop node with lowest fScore from priority queue
		pop_heap (openSet.begin(),openSet.end(), H_Compare());
		currentNode = openSet.back();
		openSet.pop_back();

		if (*currentNode == *goalNode) {
			goalNode->parent = currentNode->parent;
			break;
		}

		closedSet.push_back(currentNode);

		// Check each neighnor to currentNode - add to neighborlist if valid & not in closed set
		//Bottom-Left
		xRef = currentNode->x - 1; yRef = currentNode->y - 1;
		if (map.validCell(xRef, yRef)) {
			neighborNode = new mapNode(xRef, yRef);
			if (! inList(&closedSet,neighborNode)) {
				neighborList.push_back(neighborNode);
			}
			else delete neighborNode;
		}
		//Left
		yRef = currentNode->y;
		if (map.validCell(xRef, yRef)) {
			neighborNode = new mapNode(xRef, yRef);
			if (! inList(&closedSet,neighborNode)) {
				neighborList.push_back(neighborNode);
			}
			else delete neighborNode;
		}
		//Top-Left
		yRef = currentNode->y+1;
		if (map.validCell(xRef, yRef)) {
			neighborNode = new mapNode(xRef, yRef);
			if (! inList(&closedSet,neighborNode)) {
				neighborList.push_back(neighborNode);
			}
			else delete neighborNode;
		}
		//Top
		xRef = currentNode->x;
		if (map.validCell(xRef, yRef)) {
			neighborNode = new mapNode(xRef, yRef);
			if (! inList(&closedSet,neighborNode)) {
				neighborList.push_back(neighborNode);
			}
			else delete neighborNode;
		}
		//Bottom
		yRef = currentNode->y-1;
		if (map.validCell(xRef, yRef)) {
			neighborNode = new mapNode(xRef, yRef);
			if (! inList(&closedSet,neighborNode)) {
				neighborList.push_back(neighborNode);
			}
			else delete neighborNode;
		}
		//Bottom-Right
		xRef = currentNode->x+1;
		if (map.validCell(xRef, yRef)) {
			neighborNode = new mapNode(xRef, yRef);
			if (! inList(&closedSet,neighborNode)) {
				neighborList.push_back(neighborNode);
			}
			else delete neighborNode;
		}
		//Right
		yRef = currentNode->y;
		if (map.validCell(xRef, yRef)) {
			neighborNode = new mapNode(xRef, yRef);
			if (! inList(&closedSet,neighborNode)) {
				neighborList.push_back(neighborNode);
			}
			else delete neighborNode;
		}
		//Top-Right
		yRef = currentNode->y+1;
		if (map.validCell(xRef, yRef)) {
			neighborNode = new mapNode(xRef, yRef);
			if (! inList(&closedSet,neighborNode)) {
				neighborList.push_back(neighborNode);
			}
			else delete neighborNode;
		}

		// For each valid neighbor...
		neighborSize = neighborList.size();
		for (i = 0; i < neighborSize; i++) {
			neighborNode = neighborList.back();
			tempGScore = currentNode->gScore + costBetween(currentSquad->type, currentNode, neighborNode);

			if (! inList(&openSet, neighborNode) || tempGScore <= neighborNode->gScore) {
				neighborNode->parent = currentNode;
				neighborNode->gScore = tempGScore;
				neighborNode->hScore = neighborNode->heuristic(goalNode, HEURISTICMOD);
				neighborNode->fScore = neighborNode->gScore + neighborNode->hScore;

				if (! inList(&openSet, neighborNode)) {
					heapFlag = true;
					openSet.push_back(neighborNode);
				}

			}

			neighborList.pop_back();
		}
		if (heapFlag) {
			make_heap(openSet.begin(), openSet.end(), H_Compare());
		}

	}

	// Return empty list - no possible path
	if (openSet.empty()) {
		// Cleanup
		while (!closedSet.empty()) {
			currentNode = closedSet.back();
			delete currentNode;
			closedSet.pop_back();
		}
		
		return pathList; //will be empty - no possible path
	}


	// Construct path through list of nodes
	pathList.push_front(map.GetWorldPoint(goalNode->x, goalNode->y));
	//pathList.push_front(goalNode);
	currentNode = goalNode->parent;
	while (currentNode != 0) {
		pathList.push_front(map.GetWorldPoint(currentNode->x, currentNode->y));
		//pathList.push_front(currentNode);
		currentNode = currentNode->parent;
	}

	//Cleanup
	while (!closedSet.empty()) {
		currentNode = closedSet.back();
		delete currentNode;
		closedSet.pop_back();
	}
	while (!openSet.empty()) {
		currentNode = openSet.back();
		delete currentNode;
		openSet.pop_back();
	}

	return pathList;
}

void world::spawnResources(float dt)
{
	// If we're at the max number of resources, do nothing
	if (RESOURCE_MAX_COUNT <= resourceList.size())
	{
		resource_spawn_counter = 0;
		return;
	}

	resource_spawn_counter += dt;

	if (RESOURCE_SPAWN_DELAY <= resource_spawn_counter)
	{
		// Add new resource
		resourceList.push_back(resource());
		
		// Check it's placed somewhere valid
		bool valid = true;
		do {
			for(vector<structure *>::iterator it = structureList.begin(); it != structureList.end(); it++)
			{
				if(resourceList.back().position.distance((*it)->position) + 10 <= (*it)->radius)
				{
					valid = false;
					resourceList.back().position = vertex(randRange(-WIDTH/2.0, WIDTH/2.0), 
														  randRange(-HEIGHT/2.0, HEIGHT/2.0));
					break;
				}
			}
		} while (!valid);

		resource_spawn_counter -= RESOURCE_SPAWN_DELAY;
	}
}

bool world::attemptResourcePickup(vertex p, team_T t)
{
	for (vector<resource>::iterator it = resourceList.begin(); it != resourceList.end(); it++)
	{
		if (p.distance(it->position) <= RESOURCE_PICKUP_RADIUS)
		{
			if (BLUE == t) {
				// Add resources to the player if his unit picks up the resources
				player_resources += it->value;
				if (RESOURCE_PLAYER_MAX < player_resources)
				{
					player_resources = RESOURCE_PLAYER_MAX;
				}
			}
			resourceList.erase(it);
			return true;
		}
	}
	return false;
}

int world::destroyCleanup(FILE * outputFile) {

	int i,j;
	vector<squad*>::iterator squadIter;
	vector<agent*>::iterator agentIter;
	vector<structure*>::iterator structureIter;
	vector<void*>::iterator destroyIter;
	squad * currentSquad;
	agent * currentAgent;
	structure * currentStructure;

	squadIter = squadList.begin();

	// Check through each squad...
	while (squadIter != squadList.end()) {
		currentSquad = *squadIter;

		// Check through each agent in the squad...
		agentIter = currentSquad->agentList.begin();
		while (agentIter != currentSquad->agentList.end()) {
			currentAgent = *agentIter;

			// Cross-reference this agent against the destroyList
			destroyIter = destroyList.begin();
			while (destroyIter != destroyList.end()) {

				// If agent is marked destroyed, erase from squad list and delete from memory
				if (((void*)currentAgent) == *destroyIter) {
					// First subtract unit cost from population count
					if (currentSquad->team == BLUE) 
						bluePopulation -= currentSquad->unitPopCost;
					else
						redPopulation -= currentSquad->unitPopCost;
					// Continue with deletion
					agentIter = currentSquad->agentList.erase(agentIter);
					delete currentAgent;
					currentAgent = 0;
					break;
				}
				if (destroyIter != destroyList.end()) destroyIter++;
			}
			// Agent is not marked for destroy. Check if its referencing a destroyed target
			if (currentAgent != 0 && currentAgent->target.targetObj != 0) {
				//Cross-reference agent's target with destroyList
				for (destroyIter = destroyList.begin(); 
					 destroyIter != destroyList.end(); 
					 destroyIter++) {
						 //If the target is destroyed, reset target designation
						 if (currentAgent->target.targetObj == *destroyIter) {
							 currentAgent->target.targetObj = 0;
							 currentAgent->target.inRange = false;
							 break;
						 }
				}
			}
			// If current agent was not erased, need to manually update agentIter
			// (if agent WAS erased, the erase function already updated the iterator)
			if (currentAgent != 0) {
				fputs("Agent NOT destroyed. Updating Agent Iterator\n", outputFile);
				agentIter++;
			}
			else {
				fputs("Agent destroyed. Agent Iterator left as-is\n", outputFile);
			}
		}

		// Check if current squad is now empty. If so, erase it
		if (currentSquad->agentList.empty()) {
			squadIter = squadList.erase(squadIter);
			delete currentSquad;
			currentSquad = 0;
		}
		// Note: if squad was erased, iterator is already hovering over the next squad (or end())
		//       if squad was not erased, need to update the iterator
		else squadIter++;
	}

	int winState = 0; // Win state is 0 until someone's Citadel is destroyed
	// Now Check through each structure...
	structureIter = structureList.begin();
	while (structureIter != structureList.end()) {
		currentStructure = *structureIter;

		// Cross-reference this structure against the destroyList
		destroyIter = destroyList.begin();
		while (destroyIter != destroyList.end()) {
			// If structure is marked destroyed, erase from structure list and delete from memory
			if (((void*)currentStructure) == *destroyIter) {
				// Check if structure is a citadel
				if (structure::CITADEL == currentStructure->type)
				{
					if (RED == currentStructure->team){
						// Blue team (player) wins
						winState = 1;
					} else {
						// Red team (AI) wins
						winState = -1;
					}
				}
				//First check if its a supply depot. If so, subtract team supplies
				else if (currentStructure->type == structure::SUPPLY) {
					if (currentStructure->team == BLUE)
						blueSupplies -= SUPPLYAMMOUNT;
					else
						redSupplies -= SUPPLYAMMOUNT;
				}
				// Destroy the structure
				structureIter = structureList.erase(structureIter);
				currentStructure->evict(&map);
				delete currentStructure;
				currentStructure = 0;
				break;
			}
			if (destroyIter != destroyList.end()) destroyIter++;
		}

		// If current structure was not erased, need to manually update structureIter
		// (if structure WAS erased, the erase function already updated the iterator)
		if (currentStructure != 0) {
			fputs("Structure Not destroyed. Updating Structure Iterator\n", outputFile);
			structureIter++;
		}
		else {
			fputs("Structure destroyed. Structure Iterator left as-is\n", outputFile);
		}


		
	}

	//Clear the destroy list when all is done
	destroyList.clear();

	return winState;
}